using System;
using Microsoft.SPOT;

namespace Robot
{
    /// <summary>
    /// Implements reading of distance using optical proximity sensor and ADC
    /// </summary>
    class DistanceSensor
    {
        public delegate void DistanceChangedHandler(int value);
        /// <summary>
        /// This event is sent every time the meassured distance has changed
        /// </summary>
        public event DistanceChangedHandler DistanceChanged;

        
        System.Threading.Thread m_thread;
        int m_value;
        bool m_continue = true;
        const int DELAY = 400;

        /// <summary>
        /// Constructor
        /// </summary>
        public DistanceSensor()
        {
            m_thread = new System.Threading.Thread(new System.Threading.ThreadStart(Run));
        }

        /// <summary>
        /// Starts the readings in a thread
        /// </summary>
        public void Start()
        {
            m_continue = true;
            if (!m_thread.IsAlive)
            {
                m_thread.Start();
            }
        }

        /// <summary>
        /// Stops readings
        /// </summary>
        public void Stop()
        {
            m_continue = false;
        }

        /// <summary>
        /// Does the readings
        /// </summary>
        void Run()
        {
            while (m_continue)
            {
                int d = GetDistance();
                if (m_value != d)   // Only publish if value has changed
                {
                    m_value = d;
                    Publish();
                }

                // Wait a while before reading value again
                System.Threading.Thread.Sleep(DELAY);
            }
        }

        /// <summary>
        /// Does the hard work of publishing to every single subscriber
        /// </summary>
        void Publish()
        {
            if (DistanceChanged != null)
            {
                DistanceChanged(m_value);
            }
        }

        /// <summary>
        /// Reads and converts distance from sensor / ADC
        /// </summary>
        /// <returns>Distance to nearest object in millimetres</returns>
        public static int GetDistance()
        {
            float x = DeviceSolutions.SPOT.Hardware.TahoeII.Tsc2046.ReadBat();
            return (int)(21.762 * x * x - 124.87 * x + 210.58);
        }
    }
}
